﻿using UnityEngine;
using System.Collections;

public class PhysicSolver  {
    private float biasFactor = 0.05f;
    private float RIGIDBODY_TOLERANCE = 1e-6f;

    public void ApplyImpulse(PhysicContact contact)
    {
        var relativeVelocity = contact.A.GetVelocity() - contact.B.GetVelocity();
        float impulseMagnitude;
        Vector3 impulse;

        Vector2 k = new Vector2(contact.A.invMass, contact.B.invMass);
        //弹性系数
        var e = contact.A.Restitution * contact.B.Restitution;
        //相对速度 碰撞方向分量
        impulseMagnitude = -(1 + e) * Vector3.Dot(relativeVelocity , contact.Normal);
        //根据穿透深度 计算 修正冲量系数 弹簧根据插入深度来计算修正值
        impulseMagnitude += biasFactor * Mathf.Max(contact.PenetrationDepth, 0) / Time.fixedDeltaTime;
        //冲量除以质量
        impulseMagnitude /= k.x + k.y;

        impulse = impulseMagnitude * contact.Normal;
        contact.A.ApplyImpulse(impulse, contact.RelativeBodyPosA);
        contact.B.ApplyImpulse(-impulse, contact.RelativeBodyPosB);



        //滑动摩擦力
        var tangent = Vector3.Cross(contact.Normal, Vector3.Cross(contact.Normal, relativeVelocity));
        var tangentLen = tangent.magnitude;
        if(tangentLen > RIGIDBODY_TOLERANCE)
        {
            tangent /= tangentLen;
        } else
        {
            tangent = Vector3.zero;
        }

        k = new Vector2(contact.A.invMass, contact.B.invMass);
        if(k.x+k.y > RIGIDBODY_TOLERANCE)
        {
            impulseMagnitude = 1 / (k.x + k.y);
        }else
        {
            impulseMagnitude = 0;
        }
        var coeff = Mathf.Min(Mathf.Max(Mathf.Abs(Vector3.Dot(tangent, relativeVelocity)), 0.0f), contact.A.Friction*contact.B.Friction);
        impulse = coeff * impulseMagnitude * tangent;
        contact.A.ApplyImpulse(impulse, contact.RelativeBodyPosA);
        contact.B.ApplyImpulse(-impulse, contact.RelativeBodyPosB);


        contact.A.ApplyContact(contact);
        contact.B.ApplyContact(contact);
    }

    /// <summary>
    /// 或者根据质量修正位置
    /// </summary>
    /// <param name="contact"></param>
    public void Constraint(PhysicContact contact)
    {
        //相对速度在Constraint修正之前计算
        //var deltePos = contact.A.GetVelocity();
         
        var relativeVelocity = contact.A.GetVelocity() - contact.B.GetVelocity();
        /*
        var relativeVelocity = contact.A.GetVelocity() - contact.B.GetVelocity();

        var moveA = contact.Normal * contact.PenetrationDepth;
        var rateA = contact.A.invMass/(contact.A.invMass+contact.B.invMass);
        var rateB = contact.B.invMass/(contact.A.invMass+contact.B.invMass);
        contact.A.transform.position += moveA*rateA;
        contact.B.transform.position -= moveA * rateB;
        */
        var sphere = (SphereShapeCollider)contact.A;
        var slotLimit = sphere.SlotLimit;
        var limitCos = Mathf.Cos(Mathf.Deg2Rad*slotLimit);

        //夹角不能 > slotLimit
        var deg = Vector3.Dot(contact.Normal,Vector3.up);
        //上坡 可以多次迭代解 
        //修正 重力抵消，水平移动距离不变
        //修正移动 沿着斜坡上去 或者斜坡下去 没有条件约束
        

        //收集所有的contact 循环两次来求解 
        //上坡 迭代解constraint
        //1: 水平方向位置保证  deg > limitCost  
        //2: 水平方向不能上坡 deg <= limitCost
        //3: 所有character 相关的 contact 收集
        //4: relaxtion 迭代 解 
        //5: 地面不要穿插
        //6：相对位置保留
        //上坡 只有地面
        if(deg > limitCos)
        {
            var keepPos = sphere.transform.position;
            keepPos.y = 0;
            
            var moveA = contact.Normal * contact.PenetrationDepth;
            var tangent = Vector3.Cross(contact.Normal, Vector3.Cross(contact.Normal, relativeVelocity));
            var tangentLen = tangent.magnitude;
            if (tangentLen > RIGIDBODY_TOLERANCE)
            {
                tangent /= tangentLen;

                var fixPos1 = sphere.transform.position + moveA;
                fixPos1.y = 0;
                var deltaXZ = keepPos - fixPos1;

                var xzProj = tangent;
                xzProj.y = 0;

                var tangentMove = tangent.magnitude / xzProj.magnitude * deltaXZ.magnitude * tangent;
                sphere.transform.position += moveA + tangentMove;
                //sphere.transform.position += moveA;
            }
            else
            {
                tangent = Vector3.zero;
                sphere.transform.position += moveA;
            }

            //sphere.transform.position += moveA;
        }
        else//重力抵消不能水平移动
        {
            //保持旧的XZ 值不能变
            var keepPos = sphere.lastPosition;
            keepPos.y = 0;

            var moveA = contact.Normal * contact.PenetrationDepth;

            var tangent = Vector3.Cross(contact.Normal, Vector3.Cross(contact.Normal, relativeVelocity));
            var tangentLen = tangent.magnitude;
            if (tangentLen > RIGIDBODY_TOLERANCE)
            {
                tangent /= tangentLen;

                var fixPos1 = sphere.transform.position + moveA;
                fixPos1.y = 0;
                var deltaXZ = keepPos - fixPos1;

                var xzProj = tangent;
                xzProj.y = 0;

                var tangentMove = tangent.magnitude / xzProj.magnitude * deltaXZ.magnitude * tangent;
                sphere.transform.position += moveA + tangentMove;
            }
            else
            {
                tangent = Vector3.zero;
                sphere.transform.position += moveA;
            }
        }


        /*
        //摩擦力修正
        var tangent = Vector3.Cross(contact.Normal, Vector3.Cross(contact.Normal, relativeVelocity));
        var tangentLen = tangent.magnitude;
        if (tangentLen > RIGIDBODY_TOLERANCE)
        {
            tangent /= tangentLen;
        }
        else
        {
            tangent = Vector3.zero;
        }

        var k = new Vector2(contact.A.invMass, contact.B.invMass);
        Vector3 impulse;
        var coeff = contact.A.Friction * contact.B.Friction * contact.PenetrationDepth;
        var maxV = Mathf.Abs(Vector3.Dot(tangent, relativeVelocity));
        //Debug.Log("V" + coeff + ":"+);
        coeff = Mathf.Max(0, Mathf.Min(coeff, maxV));
        //coeff = maxV;
        //var coeff = Mathf.Min(Mathf.Max(Mathf.Abs(Vector3.Dot(tangent, relativeVelocity)), 0.0f), contact.A.Friction*contact.B.Friction);
        impulse = coeff * tangent;

        contact.A.transform.position += impulse * rateA;
        contact.B.transform.position -= impulse * rateB;
        */
    }
}
